#include <Arduino.h>
#include <logger_rte.h>

#include "Q2HX711.h"

#ifdef USE_FREERTOS

#include "STM32FreeRTOS.h"

#endif

Q2HX711::Q2HX711(byte output_pin, byte clock_pin) {
    CLOCK_PIN = clock_pin;
    OUT_PIN = output_pin;
    GAIN = 1;
    pinsConfigured = false;
}

void Q2HX711::begin() {
    //    setGain(32);
    readyToSend();
#ifdef USE_FREERTOS
    osThreadDef(pt100, this->thread, osPriorityNormal, 0, 384);
    osThreadCreate(osThread(pt100), this);
#endif
}

bool Q2HX711::readyToSend() {
    if (!pinsConfigured) {
        // We need to set the pin mode once, but not in the constructor
        pinMode(CLOCK_PIN, OUTPUT);
        pinMode(OUT_PIN, INPUT);
        pinsConfigured = true;
    }
    return digitalRead(OUT_PIN) == LOW;
}

void Q2HX711::reset() {
    digitalWrite(CLOCK_PIN, HIGH);
#ifdef USE_FREERTOS
    vTaskDelay(20);
#endif
    digitalWrite(CLOCK_PIN, LOW);
}

void Q2HX711::setGain(byte gain) {
    switch (gain) {
        case 128:
            next_gain = 1;
            break;
        case 64:
            next_gain = 3;
            break;
        case 32:
            next_gain = 2;
            break;
        default:
            break;
    }
}

uint8_t shiftInSlow(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder) {
    uint8_t value = 0;

    for (int i = 0; i < 8; ++i) {
        delayMicroseconds(1);
        digitalWrite(clockPin, HIGH);
        delayMicroseconds(1);
        if (bitOrder == LSBFIRST)
            value |= digitalRead(dataPin) << i;
        else
            value |= digitalRead(dataPin) << (7 - i);
        digitalWrite(clockPin, LOW);
    }
    return value;
}

#include "mad.h"

long Q2HX711::read() {

    byte data[4];
    uint32_t tick = millis();
#ifdef USE_FREERTOS
    vTaskSuspendAll();
#endif
    for (byte j = 3; j--;) {
        data[j] = shiftInSlow(OUT_PIN, CLOCK_PIN, MSBFIRST);
    }

    // set gain
    for (int i = 0; i < next_gain; i++) {
        delayMicroseconds(1);
        digitalWrite(CLOCK_PIN, HIGH);
        delayMicroseconds(1);
        digitalWrite(CLOCK_PIN, LOW);
    }
#ifdef USE_FREERTOS
    xTaskResumeAll();
#endif
    uint8_t filler;
    if (data[2] & 0x80) {
        data[3] = 0xFF;
    } else {
        data[3] = 0x00;
    }
    // Construct a 32-bit signed integer
    int32_t iresult = *(int32_t *) data;

    if (tick - last_tick < 250) {
        if (GAIN == 2) {
            db_buf.push_back(iresult);
            count++;
            // core_debug("db %d %d/%d", db_buf.size(), count,nrs);
            if (count >= nrs) {
                db = (int) Mad(db_buf, 0.5);
                db_buf.clear();
            }
        } else if (GAIN == 3) {
            da_buf.push_back(iresult);
            count++;
            // core_debug("da %d %d", da_buf.size(), count);
            if (count >= nrs) {
                da = (int) Mad(da_buf, 0.5);
                da_buf.clear();
            }
        }
    }
    GAIN = next_gain;
    if (count >= nrs) {
        if (next_gain == 2) {
            next_gain = 3;
        } else
            next_gain = 2;
        count = 0;
    }
    last_tick = tick;
    return static_cast<long>(iresult);
}

#ifdef USE_FREERTOS

void Q2HX711::thread(const void *_this) {
    ((Q2HX711 *) _this)->rtos_loop();
}

void Q2HX711::rtos_loop() {
    last_tick = millis();
    while (true) {
        if (readyToSend() || (millis() - last_tick > 1200)) {
            read();
        }
        vTaskDelay(1);
    }
}

#endif
